perm filename PPROC2.OLD[PNT,HE] blob
sn#516903 filedate 1980-03-27 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00012 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 ENTRY
C00005 00003 ! cmonproc
C00013 00004 ! withproc
C00021 00005 ! operproc
C00027 00006 ! arm interactions: read_pos,readarm,frasg,arm_check
C00029 00007 ! arm interactions: fconstructproc
C00033 00008 ! arm motions: movepcode,alongproc,axmovproc, pbyproc,ptoproc
C00049 00009 ! drivecode,opclcode,jtmove,driveproc
C00052 00010 ! centerproc,stopproc,retryproc
C00055 00011 ! opening, opclproc,closeproc
C00057 00012 ! onproc
C00059 ENDMK
C⊗;
ENTRY;
BEGIN "PPROC2"
DEFINE $$PRGID=TRUE;
DEFINE $PPROC2=TRUE;
DEFINE $ALTER_EGO=TRUE;
REQUIRE "HEADER.SAI" SOURCE_FILE;
RECORD_CLASS CLAUSE(RPTR(EXPR$)HEADER,HEAD0,HEAD,TAIL;
INTEGER TYPE,VALUE,CMONCODE,FBITS;
BOOLEAN WITH;REAL FVALUE);
REdefine
indices(name, postfix)"[][]"=[
redefine xxcount=1;
redefine xx(xxarg)=[
redefine xxtemp= [ define xxarg]&[postfix=xxcount];
xxtemp;
redefine xxcount=xxcount+1;];
name ];
DEFINE MOVE_ST=1,
CENTER_ST=2,
OPERATE_ST=4,
ON_ST='10,
OPEN_ST='20;
DEFINE CONDITION_INFO=[
XX(NEITHER, 0)
XX(EQUALITY, 0)
XX(RELATIONAL, 0)
XX(FORCE, MOVE_ST+ON_ST)
XX(TORQUE, MOVE_ST+ON_ST+OPERATE_ST)
XX(DURATION, MOVE_ST+ON_ST+OPERATE_ST+CENTER_ST)
XX(APPROACH, MOVE_ST)
XX(DEPARTURE, MOVE_ST)
XX(SPEED_FACTOR,MOVE_ST)
XX(FORCE_FRAME, MOVE_ST)
XX(NULLING, MOVE_ST)
XX(NO_NULLING, MOVE_ST)
XX(STIFFNESS, MOVE_ST)
XX(DRIVER_TURNS,OPERATE_ST)
XX(RTMOVE, MOVE_ST)
XX(WOBBLE, MOVE_ST)
XX(STOP_WAIT_TIME, 0)
XX(ANGULAR_VELOCITY, OPERATE_ST)
XX(FAILURE, MOVE_ST+CENTER_ST+OPERATE_ST+OPEN_ST)
XX(EXPRESSION, MOVE_ST+ON_ST+OPERATE_ST+CENTER_ST+OPEN_ST)
XX(EVENT, MOVE_ST+ON_ST+OPERATE_ST+CENTER_ST+OPEN_ST)
XX(SETBASE, MOVE_ST)
XX(DRIVER_TORQUE, OPERATE_ST)
XX(CLOCKWISE, OPERATE_ST)
XX(CCLOCKWISE, OPERATE_ST)];
INDICES(CONDITION_INFO,_COND);
define cond_count=xxcount;
REDEFINE XX(a,b)=[b,];
preload_array(VALID, CONDITION_INFO, INTEGER, 1,cond_count);
RPTR(EXPR$) PROCEDURE $RAPPEND(RPTR(RSTACK)R);
BEGIN
RTRIM(R);
RETURN($AAPPEND(RSTACK:STACK[R]));
END;
! cmonproc;
RPTR(EXPR$)PROCEDURE $FFPCODE(INTEGER DEVBITS; RPTR(EXPR$)E(NULL_RECORD));
BEGIN
RPTR(EXPR$)ARRAY F[1:2];
RPTR(SYMBOL)C;
IF E=NULL_RECORD THEN
F[1]←EXPR$3(XAGTVAL,SYMBOL:INDEX[C←CHECK("NILTRANS",#TR)],
SYMBOL:OFFSET[C])
ELSE F[1]←E;
F[2]←EXPR$2(XTFRCST,DEVBITS);
RETURN($AAPPEND(F));
END;
RPTR(EXPR$) PROCEDURE $FRCCLPCODE(RPTR(EXPR$)EXP;INTEGER BITS);
BEGIN
RPTR(EXPR$)ARRAY F[1:2];
F[1]←EXP;
F[2]←EXPR$2(XCOMPLY,BITS LAND '777377);
RETURN($AAPPEND(F));
END;
RECURSIVE RPTR(EXPR$) PROCEDURE ACTION$;
BEGIN ! checks for DO and then a statement ;
INTEGER TMPOFF; RPTR(EXPR$)E;
TMPOFF←$TMPOFF; $TMPOFF←UPLEVEL($TMPOFF);
E←RPARSE("DO");
$TMPOFF←TMPOFF;
RETURN(E);
END;
PROCEDURE VBITS(STRING ERR; REFERENCE INTEGER BITS);
BEGIN "vector directional bits"
GTOKEN;
IF EQU(TOKEN,"XHAT") THEN RETURN
ELSE IF EQU(TOKEN,"YHAT") THEN BITS←BITS LOR '1000
ELSE IF EQU(TOKEN,"ZHAT") THEN BITS←BITS LOR '2000
ELSE ERROR(ERR&" Need XHAT or YHAT or ZHAT here.");
END;
PROCEDURE RBITS(STRING ERR; REFERENCE INTEGER BITS);
BEGIN "relational bits"
GTOKEN;
IF TOKEN="≥" OR TOKEN =">" THEN BITS←BITS LOR '100000
ELSE IF TOKEN="≤" OR TOKEN="<" THEN BITS←BITS
ELSE ERROR(ERR&" need > or ≤ here");
END;
RECURSIVE PROCEDURE FORCECMON(RPTR(CLAUSE)CL;INTEGER BITOFFSET,COND;
BOOLEAN ABSOLUTE(FALSE));
BEGIN
INTEGER V; BOOLEAN GE; RPTR(EXPR$)EXP,ACTION,FR;
INTEGER I,IPC;
INTEGER BITS,DEVBITS;
RPTR(SYMBOL)C;
DEVBITS←BITOFFSET LAND '17;
BITS←BITOFFSET;
GTOKEN;
IF TOKEN="(" THEN
BEGIN
VBITS("FORCECM: ",BITS); WORD_READ(")");
IF ABSOLUTE THEN BEGIN WORD_READ("|"); BITS←BITS + '20000; END;
RBITS("FORCE CM: ",BITS);
EXP←$$GTANYEXP("FORCECM",#SC);
END
ELSE BEGIN
STOKEN←TRUE;
IF ABSOLUTE THEN BEGIN WORD_READ("|"); BITS←BITS LOR '20000; END;
RBITS("FORCE CM: ",BITS);
EXP←$$GTANYEXP("FORCECM",#SC);
WORD2_READ("ALONG","ABOUT","FORCECM: ");
VBITS("FORCECM: ",BITS)
END;
GTOKEN; FR←NULL_RECORD;
IF EQU(TOKEN,"OF") THEN
BEGIN
FR←$$GTANYEXP("FORCECM",#TR); GTOKEN;
IF EQU(TOKEN,"IN") THEN
BEGIN GTOKEN;
IF EQU(TOKEN,"HAND") THEN BITS←BITS
ELSE IF EQU(TOKEN,"STATION") THEN BEGIN BITS←BITS+'400;
DEVBITS←DEVBITS+'400; END
ELSE ERROR("FORCECM: can only specify in HAND or STATION");
END ELSE BEGIN STOKEN←TRUE; BITS←BITS+'400; DEVBITS←DEVBITS+'400; END;
WORD_READ("DO");
END
ELSE BEGIN IF NOT EQU(TOKEN,"DO") THEN ERROR("FORCECM: Need DO here");
BITS←BITS+'400; DEVBITS←DEVBITS+'400; ! default is station;
END;
STOKEN←TRUE;
ACTION←ACTION$;
CLAUSE:CMONCODE[CL]←#CMFRC;
CLAUSE:HEADER[CL]←$FRCPCODE(EXP,ACTION);
CLAUSE:FBITS[CL]←BITS;
CLAUSE:VALUE[CL]←DEVBITS;
CLAUSE:TYPE[CL]←COND;
IF FR THEN CLAUSE:HEAD0[CL]←$FFPCODE(DEVBITS,FR);
END;
RECURSIVE PROCEDURE DURCMON(RPTR(CLAUSE)CL);
BEGIN
RPTR(EXPR$)EXP,ACTION;
WORD2_READ(">","≥");
EXP←$$GTANYEXP("DURATION CMON",#SC);
ACTION←ACTION$;
CLAUSE:CMONCODE[CL]←#CMDRA;
CLAUSE:HEADER[CL]←$DURCPCODE(EXP,ACTION);
CLAUSE:TYPE[CL]←DURATION_COND;
END;
RECURSIVE PROCEDURE EXPCMON(RPTR(CLAUSE)CL);
BEGIN
RPTR(EXPR$)EXP,ACTION;
STOKEN←TRUE;
EXP←$$GTANYEXP("EXPRESSION CMON",#SC);
ACTION←ACTION$;
CLAUSE:HEADER[CL]←$EXPCPCODE(EXP,ACTION);
CLAUSE:CMONCODE[CL]←#CMEXP;
CLAUSE:TYPE[CL]←EXPRESSION_COND;
END;
RECURSIVE PROCEDURE EVCMON(RPTR(CLAUSE)CL);
BEGIN
RPTR(EXPR$)EXP,ACTION; RPTR(SYMBOL)SYM;
STOKEN←TRUE;
EXP←$$GTIDREF(#EV,SYM,"EVENT CMON");
ACTION←ACTION$;
CLAUSE:HEADER[CL]←$EVCPCODE(EXP,ACTION);
CLAUSE:CMONCODE[CL]←#CMEVT;
CLAUSE:TYPE[CL]←EVENT_COND;
END;
RECURSIVE PROCEDURE CMONPROC(INTEGER STATEMENT_TPYE;
RPTR(CLAUSE)CL;INTEGER BITS(BARM_MECH));
BEGIN
INTEGER NBITS; BOOLEAN SAVERRORCMON;
$COMPILE←$COMPILE+1;
GTOKEN;
SAVERRORCMON←$ERRCMON; $ERRCMON←FALSE; $ERRLEVEL←$LEVEL;
IF EQU(TOKEN,"ERROR") THEN
BEGIN
$ERRCMON←TRUE;
CLAUSE:WITH[CL]←TRUE; ! actually a WITH ;
WORD_READ("=");
CLAUSE:FVALUE[CL]←$GTREAL("ERROR condition monitor");
CLAUSE:TYPE[CL]←FAILURE_COND;
CLAUSE:TAIL[CL]←RPARSE("DO");
GTOKEN(FALSE);
END
ELSE
BEGIN
IF TOKEN="|" THEN
BEGIN GTOKEN;
IF EQU(TOKEN,"FORCE") THEN FORCECMON(CL,BITS,FORCE_COND,TRUE)
ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECMON(CL,BITS+'3000,
TORQUE_COND,TRUE)
ELSE ERROR("Must have FORCE or TORQUE after |");
END
ELSE IF EQU(TOKEN,"FORCE") THEN FORCECMON(CL,BITS,FORCE_COND)
ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECMON(CL,BITS+'3000,TORQUE_COND)
ELSE IF EQU(TOKEN,"DURATION") THEN DURCMON(CL)
ELSE IF (#TOKEN=ID_TYPE) AND (SYMBOL:TYPE[TOKENPTR]=#EV) THEN EVCMON(CL)
ELSE EXPCMON(CL);
CLAUSE:HEADER[CL]←$CMONPCODE(CLAUSE:HEADER[CL],CLAUSE:CMONCODE[CL],
CLAUSE:FBITS[CL]);
CLAUSE:HEAD[CL]←$PCD11(XXCMENBL,$TMPOFF);
CLAUSE:TAIL[CL]←$PCD11(XXCMDSBL,$TMPOFF);
$TMPOFF←$TMPOFF+1;
GTOKEN(FALSE);
END;
$ERRCMON←SAVERRORCMON; $ERRLEVEL←$LEVEL;
$COMPILE←$COMPILE-1;
END;
! withproc;
PROCEDURE FORCECL(RPTR(CLAUSE)CL;INTEGER BITOFFSET,COND);
BEGIN
INTEGER V; RPTR(EXPR$)EXP,FR;
INTEGER I,IPC;
INTEGER BITS,DEVBITS,TMPOFF;
RPTR(SYMBOL)C;
DEVBITS←BITOFFSET LAND '17;
BITS←BITOFFSET;
GTOKEN;
IF TOKEN="(" THEN
BEGIN
VBITS("FORCE CLAUSE: ",BITS);
WWORD_READ(")","=");
EXP←$$GTANYEXP("FORCE COMPLIANCE",#SC);
END
ELSE IF TOKEN = "=" THEN
BEGIN
EXP←$$GTANYEXP("FORCE COMPLIANCE",#SC);
GTOKEN;
IF EQU(TOKEN,"ALONG") OR EQU(TOKEN,"ABOUT") THEN
VBITS("FORCE CLAUSE: ",BITS)
ELSE ERROR("Need ALONG or ABOUT here");
END
ELSE ERROR("Need ( here ");
GTOKEN(FALSE);
FR←NULL_RECORD;
IF EQU(TOKEN,"OF") THEN
BEGIN
FR←$$GTANYEXP("FORCE CLAUSE",#TR);
GTOKEN(FALSE);
IF EQU(TOKEN,"IN") THEN
BEGIN GTOKEN;
IF EQU(TOKEN,"HAND") THEN BITS←BITS
ELSE IF EQU(TOKEN,"FIXED") THEN
BEGIN BITS←BITS+'400; DEVBITS←DEVBITS+'400; END
ELSE ERROR("FORCECM: can only specify in HAND or STATION");
END ELSE BEGIN STOKEN←TRUE; BITS←BITS+'400;
DEVBITS←DEVBITS+'400; END;
END
ELSE BEGIN
STOKEN←TRUE;
BITS←BITS+'400; DEVBITS←DEVBITS+'400; ! default is station;
END;
CLAUSE:HEAD[CL]←$FRCCLPCODE(EXP,BITS);
CLAUSE:VALUE[CL]←DEVBITS;
CLAUSE:TYPE[CL]←COND;
IF FR THEN CLAUSE:HEAD0[CL]←$FFPCODE(DEVBITS,FR);
END;
RECURSIVE PROCEDURE WITHPROC(INTEGER STATEMENT_TYPE;
RPTR(CLAUSE)CL; INTEGER BITS(BARM_MECH));
BEGIN
$COMPILE←$COMPILE+1;
CLAUSE:WITH[CL]←TRUE;
GTOKEN;
IF EQU(TOKEN,"FORCE_WRIST") THEN
BEGIN BOOLEAN NOBASE; NOBASE←FALSE;
GTOKEN;
IF EQU(TOKEN,"NOT") THEN BEGIN NOBASE←TRUE; GTOKEN; END;
IF NOT EQU(TOKEN,"ZEROED")
THEN ERROR("FORCE_WRIST CLAUSE:: must be ZEROED or NOT ZEROED");
IF ¬NOBASE THEN CLAUSE:HEAD[CL]←$PCD1(XXSETBAS);
CLAUSE:TYPE[CL]←SETBASE_COND;
END
ELSE IF EQU(TOKEN,"STIFFNESS") THEN
BEGIN
WORD_READ("=");
SETSTIFFPROC;
CLAUSE:HEAD[CL]←$$PCODE;
CLAUSE:TYPE[CL]←STIFFNESS_COND;
END
ELSE IF EQU(TOKEN,"WOBBLE") THEN
BEGIN
WORD_READ("=");
CLAUSE:FVALUE[CL]←$GTREAL("WOBBLE command");
IF (CLAUSE:FVALUE[CL]<0) OR (CLAUSE:FVALUE[CL]>30)
THEN ERROR("WOBBLE MAGNITUDE must be between 0 and 30");
CLAUSE:TYPE[CL]←WOBBLE_COND;
END
ELSE IF EQU(TOKEN,"DURATION") THEN
BEGIN
WORD_READ("=");
CLAUSE:TYPE[CL]←DURATION_COND;
IF STATEMENT_TYPE=OPERATE_ST
THEN CLAUSE:HEAD[CL]←$$GTANYEXP("DURATION",#SC)
ELSE CLAUSE:FVALUE[CL]←$GTREAL("DURATION command")
END
ELSE IF EQU(TOKEN,"FORCE") THEN FORCECL(CL,BITS,FORCE_COND)
ELSE IF EQU(TOKEN,"TORQUE") AND STATEMENT_TYPE=OPERATE_ST THEN
BEGIN
WORD_READ("=");
CLAUSE:TYPE[CL]←DRIVER_TORQUE_COND;
CLAUSE:HEAD[CL]←$$GTANYEXP("DRIVER_TORQUE",#SC);
END
ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECL(CL,BITS+'3000,TORQUE_COND)
ELSE IF EQU(TOKEN,"NULLING") THEN CLAUSE:TYPE[CL]←NULLING_COND
ELSE IF EQU(TOKEN,"NO_NULLING") THEN CLAUSE:TYPE[CL]←NO_NULLING_COND
ELSE IF EQU(TOKEN,"CLOCKWISE") THEN CLAUSE:TYPE[CL]←CLOCKWISE_COND
ELSE IF EQU(TOKEN,"COUNTER_CLOCKWISE") THEN CLAUSE:TYPE[CL]←CCLOCKWISE_COND
ELSE IF EQU(TOKEN,"ANGULAR_VELOCITY") THEN
BEGIN
WORD_READ("=");
CLAUSE:TYPE[CL]←ANGULAR_VELOCITY_COND;
CLAUSE:HEAD[CL]←$$GTANYEXP("ANGULAR_VELOCITY",#SC);
END
ELSE IF EQU(TOKEN,"SPEED_FACTOR") THEN
BEGIN
WORD_READ("=");
CLAUSE:FVALUE[CL]←$GTREAL("SPEED_FACTOR command");
CLAUSE:TYPE[CL]←SPEED_FACTOR_COND;
END
ELSE IF EQU(TOKEN,"ARRIVAL") THEN
BEGIN
WORD_READ("="); GTOKEN;
IF EQU(TOKEN,"NILDEPROACH") THEN CLAUSE:VALUE[CL]←-1
ELSE BEGIN STOKEN←TRUE; CLAUSE:HEAD[CL]←$$GTEXPR; END;
CLAUSE:TYPE[CL]←APPROACH_COND;
END
ELSE IF EQU(TOKEN,"DEPARTURE") THEN
BEGIN
WORD_READ("="); GTOKEN;
IF EQU(TOKEN,"NILDEPROACH") THEN CLAUSE:VALUE[CL]←-1
ELSE BEGIN STOKEN←TRUE; CLAUSE:HEAD[CL]←$$GTEXPR; END;
CLAUSE:TYPE[CL]←DEPARTURE_COND;
END
ELSE IF EQU(TOKEN,"FORCE_FRAME") THEN
BEGIN
WORD_READ("=");
CLAUSE:TYPE[CL]←FORCE_FRAME_COND;
CLAUSE:HEAD[CL]←$$GTANYEXP("FORCE FRAME",#TR);
GTOKEN(FALSE);
IF EQU(TOKEN,"IN") THEN
BEGIN GTOKEN;
IF EQU(TOKEN,"STATION") THEN CLAUSE:VALUE[CL]←'400
ELSE IF NOT EQU(TOKEN,"HAND") THEN ERROR("FORCE_FRAME: Need STATION or HAND here");
END ELSE STOKEN←TRUE;
END
ELSE ERROR("WITH: cannot currently handle "&TOKEN);
GTOKEN(FALSE);
$COMPILE←$COMPILE-1;
END;
! operproc;
RPTR(EXPR$)PROCEDURE $OPERPCODE;
BEGIN DEFINE CBITS=0,EBITS=0,RADDR=-2,NADDR=6;
INTEGER I;
FOR I←XPOPERATE,DRIVERSB,CBITS,DRIVER_MECH,EBITS,NADDR,RADDR
DO IPUSH(I);
RETURN(βEXPR$);
END;
RPTR(EXPR$)RECURSIVE PROCEDURE FULLOPER(RPTR(CLAUSE)ARRAY CLAUSES; INTEGER #CLAUSES);
BEGIN
INTEGER I,#NEWVARS;
INTEGER #CWS,#TORQVELS,#DURS,#ERRS;
BOOLEAN CCW;
RPTR(EXPR$)TORQ_EXP,VEL_EXP,DUR_EXP,OPERCODE,ERR_EXP;
RPTR(RSTACK)HR,H,T;
RPTR(EXPR$)HHR,HH,TT;
HR←NEW_RSTACK;
H←NEW_RSTACK;
T←NEW_RSTACK;
#ERRS←#CWS←#TORQVELS←#DURS←#NEWVARS←0;
CCW←FALSE;
DUR_EXP←$PCD11(XXPUSHINTI,2);
VEL_EXP←$PCD11(XXPUSHINTI,0);
TORQ_EXP←$PCD11(XXPUSHINTI,0);
OPERCODE←$OPERPCODE;
FOR I←1 STEP 1 UNTIL #CLAUSES DO
IF CLAUSE:WITH[CLAUSES[I]] THEN
CASE CLAUSE:TYPE[CLAUSES[I]] OF
BEGIN
[CLOCKWISE_COND]
IF #CWS THEN ERROR("Can only specify CW or CCW once")
ELSE BEGIN #CWS←#CWS+1; CCW←FALSE; END;
[CCLOCKWISE_COND]
IF #CWS THEN ERROR("Can only specify CW or CCW once")
ELSE BEGIN #CWS←#CWS+1; CCW←TRUE; END;
[ANGULAR_VELOCITY_COND]
IF #TORQVELS THEN ERROR("Can only specify TORQUE or VELOCITY once")
ELSE BEGIN #TORQVELS←#TORQVELS+1; VEL_EXP←CLAUSE:HEAD[CLAUSES[I]];END;
[DRIVER_TORQUE_COND]
IF #TORQVELS THEN ERROR("Can only specify TORQUE or VELOCITY once")
ELSE BEGIN #TORQVELS←#TORQVELS+1; TORQ_EXP←CLAUSE:HEAD[CLAUSES[I]]; END;
[DURATION_COND]
IF #DURS THEN ERROR("Can only specify one duration CLAUSE")
ELSE BEGIN #DURS←#DURS+1; DUR_EXP←CLAUSE:HEAD[CLAUSES[I]]; END;
[FAILURE_COND]
IF #ERRS THEN ERROR("FAILURE condition can only occur once")
ELSE
BEGIN
INTEGER J;
J←EXPR$:#BODY[OPERCODE];
EXPR$:BODY[OPERCODE][J-2]←CLAUSE:FVALUE[CLAUSES[I]];
ERR_EXP←CLAUSE:TAIL[CLAUSES[I]];
EXPR$:BODY[OPERCODE][J-1]← 5 + (EXPR$:#BODY[ERR_EXP]+1);
END;
ELSE ERROR("Unexpected clause found , clause no. "&cvs(I))
END
ELSE BEGIN "cmons"
#NEWVARS←#NEWVARS+1;
CASE CLAUSE:TYPE[CLAUSES[I]] OF
BEGIN
[DURATION_COND][EXPRESSION_COND][EVENT_COND]
BEGIN RPUSH(HR,CLAUSE:HEADER[CLAUSES[I]]);
RPUSH(H,CLAUSE:HEAD[CLAUSES[I]]);
RPUSH(T,CLAUSE:TAIL[CLAUSES[I]]);
END;
ELSE ERROR("Invalid clause for operate")
END;
END "cmons";
BEGIN
RPTR(EXPR$) ARRAY OP[1:12];
OP[1]←IF RSIZE(HR) THEN $RAPPEND(HR) ELSE $PCD1(XXNOOP);
OP[2]←IF RSIZE(H) THEN $RAPPEND(H) ELSE $PCD1(XXNOOP);
OP[3]←$PCD1(XXPUSHPC);
OP[4]←VEL_EXP;
OP[5]←IF CCW THEN $PCD1(XXSNEG) ELSE $PCD1(XXNOOP);
OP[6]←TORQ_EXP;
OP[7]←OP[5];
OP[8]←DUR_EXP;
OP[9]←OPERCODE;
OP[10]←ERR_EXP;
OP[11]←$PCD1(XXMDONE);
OP[12]←$PCD11(XXPKVAR,#NEWVARS);
EXPR$:BODY[OPERCODE][EXPR$:#BODY[OPERCODE]]← -EXPR$OFF(OP,4,8);
$$PCODE←$AAPPEND(OP);
END;
END;
INTERNAL RECURSIVE PROCEDURE OPERPROC;
BEGIN
RPTR(CLAUSE)ARRAY CLAUSES[1:15]; RPTR(CLAUSE)C;
INTEGER #CLAUSES;
#CLAUSES←0;
WORD2_READ("DRIVER","VISE");
IF EQU(TOKEN,"VISE") THEN ERROR("VISE not operable yet");
GTOKEN;
WHILE EQU(TOKEN,"CLOCKWISE") OR EQU(TOKEN,"COUNTER_CLOCKWISE") OR
EQU(TOKEN,"WITH") OR EQU(TOKEN,"ON") DO
BEGIN
C←NEW_RECORD(CLAUSE);
IF EQU(TOKEN,"CLOCKWISE") OR EQU(TOKEN,"COUNTER_CLOCKWISE") THEN
BEGIN
STOKEN←TRUE;
WITHPROC(OPERATE_ST,C);
END
ELSE IF EQU(TOKEN,"WITH") THEN WITHPROC(OPERATE_ST,C)
αIELSE CMONPROC(OPERATE_ST,C);
CLAUSES[#CLAUSES←#CLAUSES+1]←C;
END;
GTOKEN(FALSE);
$$PCODE←FULLOPER(CLAUSES,#CLAUSES);
END;
! arm interactions: read_pos,readarm,frasg,arm_check;
IFC FALSE THENC
! assigns the value of pos(pointer or arm) to the frame fra. If direct
is indicated uses it to set the rotation part;
! returns the pointer to the input device pos (arm or pointer);
RPTR (FRAME) PROCEDURE INPT_DEV(REFERENCE STRING POS);
BEGIN
RPTR(FRAME) FROM;
IF EQU(POS,"BARM")
THEN RETURN(F_BARM)
ELSE IF EQU(POS,"YARM")
THEN RETURN(F_YARM)
ELSE BEGIN
FROM←BELONGS(POS,#FR);
WHILE FROM≠F_BARM AND FROM≠F_YARM
DO BEGIN
PRINT("reading on arm required");
POS←RECOVER(POS);
FROM←BELONGS (POS,#FR);
END;
RETURN(FROM);
END;
END;
! reads the position of the arm from, or of the arm with pointer;
PROCEDURE READ_DEV(RPTR(FRAME) FROM);
print("dummy call to get value of the frame");
! reads the position of the device pos (arm or pointer);
PROCEDURE INPT(REFERENCE STRING POS);
BEGIN
RPTR(FRAME)FROM;
FROM←INPT_DEV(POS);
READ_DEV(FROM);
END;
ENDC
! arm interactions: fconstructproc;
! reads an axis name and returns its number:
xhat=0,yhat=1,zhat=2;
IFC FALSE THENC
INTEGER PROCEDURE INPT_AXIS(REFERENCE STRING AXIS);
WHILE TRUE DO
BEGIN
AXIS←RECOVER(AXIS);
IF EQU(AXIS[2 TO ∞],"HAT") THEN RETURN(AXIS - "X")
ELSE PRINT("--→ XHAT or YHAT or ZHAT required ←--",
CRLF,"Try again ");
END;
RPTR(TRANS) ARRAY T_CSTR[1:3];
! used by CONSTRUCT instruction;
! performs a construct instruction, without arguments;
PROCEDURE FCONSTRUCTPROC;
BEGIN
RPTR(FRAME) ELF;RPTR(TRANS)XFE;INTEGER I;
RPTR(FRAME) FROM;STRING POS,ANSWER,FIRST;
RPTR(VECTOR) V1,V2,V3;
PRELOAD_WITH
"move arm to the origin of the frame"&CRLF,
"move arm to the axis ",
"move arm to the plane ";
OWN STRING ARRAY INFORM[1:3];
STRING AXIS;INTEGER F_AXIS,S_AXIS;
$ALLOW←$ALLOW+1;
GTOKEN;
IF #TOKEN≠UNDECLARED_TYPE THEN ERROR("Need undeclared token for FCONSTRUCT")
ELSE FIRST←TOKEN;
AXIS←NULL;
IF F_POINTER=NULL_RECORD
THEN PRINT("pointer is not defined cannot be used",CRLF)
ELSE POS←"POINTER";
PRINT("three positions are required",CRLF);
FOR I←1 STEP 1 UNTIL 3 DO
BEGIN
! determination of the input device required;
PRINT("position ",I," read on ");
POS←RECOVER(POS);
FROM←INPT_DEV(POS); ! checks the input device;
! determination of the positions for reading;
PRINT(INFORM[I]);
IF I=2
THEN F_AXIS←INPT_AXIS(AXIS)
ELSE IF I=3
THEN BEGIN
PRINT(AXIS," - ");
AXIS←NULL;
S_AXIS←INPT_AXIS(AXIS);
IF S_AXIS=F_AXIS THEN ERROR("instruction not executed");
END;
! reading of the arm position;
PRINT("type <cr> when the arm is at the desired position");
ANSWER←INCHRW;
IF ANSWER=CR
THEN ANSWER←INCHRW
ELSE ERROR("instruction not executed");
READ_DEV(FROM); ! raads the appropriate arm pos.;
T_CSTR[I]←ABSLOC(FROM);
END;
! extraction of translation part;
V1←TPOS(T_CSTR[1]);
V2←TPOS(T_CSTR[2]);
V3←TPOS(T_CSTR[3]);
XFE←VVVTR(V1,V2,V3,F_AXIS,S_AXIS);
ELF←FR_INSERT(FIRST); ! inserts the new frame;
ABSSET(ELF,XFE); ! sets the new value;
$ALLOW←$ALLOW-1;
IFC #DISPL THENC UPDATE;ENDC
END;
ENDC
! arm motions: movepcode,alongproc,axmovproc, pbyproc,ptoproc
moveproc, parkingproc;
RECURSIVE RPTR(EXPR$)PROCEDURE FULLMOVE(RPTR(CLAUSE)ARRAY CLAUSES;
INTEGER #CLAUSES; RPTR(EXPR$)MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL);
BEGIN RPTR(RSTACK)HR,H,T;
RPTR(EXPR$)HHR,HH,TT,FFRAME;
RPTR(CLAUSE)FAILURE_CLAUSE;
INTEGER I,#NEWVAR,DEVBITS;
INTEGER STIFFS,COMPLYS,FORCE_FRAMES,NULLS,SETBASES,DURS,WOBBLES;
! counters ;
STIFFS←COMPLYS←FORCE_FRAMES←NULLS←SETBASES←DURS←WOBBLES←0;
HR←NEW_RSTACK;
H←NEW_RSTACK;
T←NEW_RSTACK;
#NEWVAR←0;
FOR I←1 STEP 1 UNTIL #CLAUSES DO
IF CLAUSE:WITH[CLAUSES[I]] THEN
CASE CLAUSE:TYPE[CLAUSES[I]] OF
BEGIN
[SETBASE_COND]
IF SETBASES=0 THEN
BEGIN SETBASES←SETBASES+1;
RPUSH(H,CLAUSE:HEAD[CLAUSES[I]]);
END
ELSE ERROR("ONLY one WRIST zeroed or non-zeroed allowed");
[STIFFNESS_COND]
BEGIN
RPUSH(H,CLAUSE:HEAD[CLAUSES[I]]);
STIFFS←STIFFS+1;
END;
[NO_NULLING_COND]
IF NULLS=0 THEN
BEGIN
EXPR$:BODY[MOVECODE][5]←EXPR$:BODY[MOVECODE][5] LOR 1;
NULLS←NULLS+1;
END
ELSE ERROR("ONLY one NULLING condition allowed");
[NULLING_COND]
IF NULLS=0 THEN
BEGIN
EXPR$:BODY[MOVECODE][5]←EXPR$:BODY[MOVECODE][5]
LAND '777776;
NULLS←NULLS+1;
END
ELSE ERROR("ONLY one NULLING condition allowed");
[DURATION_COND]
IF DURS=0 THEN
BEGIN
EXPR$:BODY[MOVECODE][7]←CLAUSE:FVALUE[CLAUSES[I]]*1000;
DURS←DURS+1;
END
ELSE ERROR("ONLY one DURATION or SPEED_FACTOR allowed");
[SPEED_FACTOR_COND]
IF DURS=0 THEN
BEGIN
EXPR$:BODY[MOVECODE][7]←-CLAUSE:FVALUE[CLAUSES[I]]*1000;
DURS←DURS+1;
END
ELSE ERROR("ONLY one DURATION or SPEED_FACTOR allowed");
[WOBBLE_COND]
IF WOBBLES=0 THEN
BEGIN
EXPR$:BODY[MOVECODE][6]←CLAUSE:FVALUE[CLAUSES[I]]*1000;
EXPR$:BODY[MOVECODE][5]←EXPR$:BODY[MOVECODE][5] LOR 2;
WOBBLES←WOBBLES+1;
END
ELSE ERROR("ONLY one WOBBLE command allowed");
[FORCE_FRAME_COND]
IF FFRAME THEN ERROR("Defining FORCE FRAME more than once")
ELSE
FFRAME←$FFPCODE(CLAUSE:VALUE[CLAUSES[I]],
CLAUSE:HEAD[CLAUSES[I]]);
[FORCE_COND][TORQUE_COND]
BEGIN
COMPLYS←COMPLYS+1;
RPUSH(H,CLAUSE:HEAD[CLAUSES[I]]);
IF CLAUSE:HEAD0[CLAUSES[I]] THEN
IF FFRAME THEN ERROR("Defining force frame more than once")
ELSE FFRAME←CLAUSE:HEAD0[CLAUSES[I]];
DEVBITS←DEVBITS LOR CLAUSE:VALUE[CLAUSES[I]];
FORCE_FRAMES←FORCE_FRAMES+1;
END;
[DEPARTURE_COND][APPROACH_COND]
IF CLAUSE:VALUE[CLAUSES[I]]≠-1 THEN
PRINT(CRLF&"DEPARTURE or APPROACH: be warned that they don't work");
[FAILURE_COND]
BEGIN
INTEGER J;
J←EXPR$:#BODY[MOVECODE];
EXPR$:BODY[MOVECODE][J-2]←CLAUSE:FVALUE[CLAUSES[I]];
FAILURE_CLAUSE←CLAUSES[I];
EXPR$:BODY[MOVECODE][J-1]←
5+EXPR$:#BODY[CLAUSE:TAIL[FAILURE_CLAUSE]];
END;
ELSE
END
ELSE
BEGIN RPUSH(HR,CLAUSE:HEADER[CLAUSES[I]]);
RPUSH(H,CLAUSE:HEAD[CLAUSES[I]]);
RPUSH(T,CLAUSE:TAIL[CLAUSES[I]]);
#NEWVAR←#NEWVAR+1;
IF CLAUSE:TYPE[CLAUSES[I]]=FORCE_COND
OR CLAUSE:TYPE[CLAUSES[I]]=TORQUE_COND
THEN BEGIN FORCE_FRAMES←FORCE_FRAMES+1;
DEVBITS←DEVBITS LOR CLAUSE:VALUE[CLAUSES[I]];
IF CLAUSE:HEAD0[CLAUSES[I]]
THEN IF FFRAME THEN ERROR("Defining force frame more than once")
ELSE FFRAME←$PCD1(XXNOOP);
END;
END;
IF (COMPLYS>0) AND (STIFFS=0) THEN RPUSH(H,$PCD1(XXSTIF0));
IF (SETBASES=0) AND ((COMPLYS>0) OR (#NEWVAR>0)) THEN RPUSH(H,$PCD1(XXSETBAS));
IF RSIZE(H)
THEN HH←$APPEND($RAPPEND(H),MOVECODE)
ELSE HH←MOVECODE;
IF FORCE_FRAMES THEN
IF FFRAME THEN
BEGIN
EXPR$:BODY[FFRAME][EXPR$:#BODY[FFRAME]]←
EXPR$:BODY[FFRAME][EXPR$:#BODY[FFRAME]] LOR DEVBITS;
HH←$APPEND(FFRAME,HH)
END
ELSE HH←$APPEND($FFPCODE(DEVBITS),HH);
EXPR$:BODY[HH][I←EXPR$:#BODY[HH]] ←5-I; ! retry addr;
IF FAILURE_CLAUSE THEN HH←$APPEND(HH,CLAUSE:TAIL[FAILURE_CLAUSE]);
HH←$APPEND($PCD1(XXPUSHPC),HH);
IF RSIZE(T) THEN TT←$RAPPEND(T);
IF RSIZE(HR)
THEN HHR←$APPEND($RAPPEND(HR),HH)
ELSE HHR←HH;
BEGIN
RPTR(EXPR$)ARRAY TMP[1:7];
TMP[1]←MOVEDEC;
TMP[2]←DESTCOMP;
TMP[3]←HHR;
TMP[4]←TT;
TMP[5]←$PCD1(XXMDONE);
TMP[6]←$PCD11(XXPKVAR,#NEWVAR);
TMP[7]←MOVEKIL;
RETURN($AAPPEND(TMP));
END;
END;
! returns the pointer to the arm affixed to obj;
RPTR(FRAME) PROCEDURE ARM_CHECK(RPTR(FRAME) OBJ);
BEGIN
RPTR(FRAME) TEMP;
TEMP←OBJ;
WHILE TEMP≠F_WRLD DO
IF EQU(FRAME:PNAME[TEMP],"BARM") THEN RETURN(TEMP)
ELSE IF EQU(FRAME:PNAME[TEMP],"YARM") THEN ERROR("YARM cannot be moved")
ELSE TEMP←FRAME:DAD[TEMP];
ERROR(FRAME:PNAME[OBJ]," cannot be moved");
END;
! saves the first part of the instruction for move commands;
PROCEDURE OLDSAV(STRING CMD,OBJ);
BEGIN
OLDCMD←CMD;
OLDOBJ←OBJ;
END;
PROCEDURE MOVEPCODE(RPTR(FRAME) MFRAME;
RPTR(EXPR$) ARRAY FDESTS; INTEGER NFDEST;
REFERENCE RPTR(EXPR$)MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL);
BEGIN
RPTR(SYMBOL) S1,S2; RPTR(FRAME)F1; INTEGER NFDEST0;
S1←CHECK(FRAME:PNAME[MFRAME],#FR);
S2←CHECK(FRAME:PNAME[F1←ARM_CHECK(MFRAME)],#FR);
$TTROFF←$TMPOFF;
NFDEST0←NFDEST+1;
$TMPOFF←$TMPOFF+NFDEST0;
$MOVEPCODE(S1,S2,FDESTS,NFDEST,DESTCOMP,MOVECODE);
MOVEDEC←$SMPDCLPCODE(#TR,NFDEST0);
MOVEKIL←$PCD11(XXPKVAR,NFDEST0);
END;
INTERNAL PROCEDURE ALONGPROC(STRING AXIS,FRA1);
BEGIN
INTEGER I,INDEX;
RPTR(expr$)SCAL;RPTR(SYMBOL)SYMPTR;RPTR(FRAME)FRAM1;
INTEGER ARRAY BUFF1[1:3],BUFF3[1:5];
RPTR(EXPR$)ARRAY PTR[1:3],DEST[1:1];
SCAL←$$GTANYEXP("distance to be moved along axis",#SC);
SYMPTR←CHECK(AXIS[1 TO 1]&"HAT",#VT);
OLDSAV("MOVE"&AXIS[1 TO 1],FRA1); ! saves for default instructions;
FRAM1←BELONGS(FRA1,#FR);
INDEX←0;
FOR I←XAGTVAL, SYMBOL:INDEX[SYMPTR],SYMBOL:OFFSET[SYMPTR],
XSVMUL, XTVADD DO BUFF3[INDEX←INDEX+1]←I;
SYMPTR←CHECK(FRA1,#FR);
INDEX←0;
IF SYMBOL:INDEX[SYMPTR]>0 THEN
FOR I←XAGTVAL, SYMBOL:INDEX[SYMPTR],SYMBOL:OFFSET[SYMPTR]
DO BUFF1[INDEX←INDEX+1]←I
ELSE FOR I←XGTVAL, SYMBOL:OFFSET[SYMPTR],XNOOP
DO BUFF1[INDEX←INDEX+1]←I;
PTR[1]←αEXPR$(BUFF1,0);
PTR[2]←SCAL;
PTR[3]←αEXPR$(BUFF3,0);
DEST[1]←$AAPPEND(PTR);
BEGIN RPTR(EXPR$)ARRAY M[1:4];
MOVEPCODE(FRAM1,DEST,1,M[1],M[2],M[3],M[4]);
$$PCODE←$AAPPEND(M);
END;
$DISPLAYLIST[#FR]←NULL;
END;
! moves the frame along one axis by a scalar;
INTERNAL PROCEDURE AXMOVPROC;
BEGIN
STRING FRA1,AXIS;
AXIS←TOKEN[5 TO 5];
FRA1←MVFR_READ;
WORD_READ("BY");
ALONGPROC(AXIS,FRA1);
$DISPLAYLIST[#FR]←NULL;
END;
! reads/exec TO <fr>+<vt>{wrt <fr>} or BY <vector>{wrt <fr>};
PROCEDURE PPBYPROC(REFERENCE RPTR(EXPR$)D,C,M,K);
BEGIN
RPTR(EXPR$)ARRAY E[1:4];
RPTR(FRAME) FRAM1;RPTR(EXPR$)ARRAY FDEST[1:1];
! MOVE<fr>BY<vt> ≡ MOVE<fr>TO⊗+<vt>;
TOKEN←OLDOBJ;
#TOKEN←ID_TYPE;
STOKEN←TRUE;
$CLINR←"+"&$CLINR;
FDEST[1]←$$GTANYEXP("destination of MOVE",#FR);
FRAM1←BELONGS (OLDOBJ,#FR);
MOVEPCODE(FRAM1,FDEST,1,D,C,M,K);
$DISPLAYLIST[#FR]←NULL;
E[1]←D;E[2]←C;E[3]←M;E[4]←K;
$$PCODE←$AAPPEND(E);
END;
PROCEDURE PPTOPROC(REFERENCE RPTR(EXPR$)D,C,M,K);
BEGIN
RPTR(FRAME) FRAM1; RPTR(EXPR$) ARRAY FDESTS[1:10]; INTEGER NFDEST;
RPTR(EXPR$)ARRAY E[1:4];
NFDEST←0;
DO BEGIN
FDESTS[NFDEST←NFDEST+1]←$$GTANYEXP("Destination part of MOVE",#FR);
IF NFDEST=10 THEN ERROR("Pointy cannot currently handle more than a 9 segment move");
GTOKEN(FALSE);
END UNTIL TOKEN≠",";
STOKEN←TRUE;
FRAM1←BELONGS (OLDOBJ,#FR);
MOVEPCODE(FRAM1,FDESTS,NFDEST,D,C,M,K);
$DISPLAYLIST[#FR]←NULL;
E[1]←D;E[2]←C;E[3]←M;E[4]←K;
$$PCODE←$AAPPEND(E);
END;
INTERNAL PROCEDURE PBYPROC(REFERENCE RPTR(EXPR$)C,M);
BEGIN RPTR(EXPR$) D,K; PPBYPROC(D,C,M,K); END;
INTERNAL PROCEDURE PTOPROC(REFERENCE RPTR(EXPR$)C,M);
BEGIN RPTR(EXPR$) D,K; PPTOPROC(D,C,M,K); END;
INTERNAL RECURSIVE PROCEDURE MOVEPROC;
BEGIN RPTR(EXPR$)MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL; STRING FR1,AXIS;
INTEGER TMPOFF0; BOOLEAN ERRCMON_SEEN;
FR1←IDF_READ; GTOKEN;
OLDSAV("MOVE",FR1);
TMPOFF0←$TMPOFF; ERRCMON_SEEN←FALSE;
IF EQU(TOKEN,"TO") THEN PPTOPROC(MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL)
ELSE IF EQU(TOKEN,"BY") THEN PPBYPROC(MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL)
ELSE ERROR("TO or BY required");
GTOKEN(FALSE);
IF EQU(TOKEN,"ON") OR EQU(TOKEN,"WITH")THEN
BEGIN "on or with"
RPTR(CLAUSE)ARRAY CLAUSES[1:15]; INTEGER #CLAUSES;
INTEGER BITS,TMPOFF;
TMPOFF←$TMPOFF; #CLAUSES←0;
IF EQU(FR1,"BARM") THEN BITS←BARM_MECH ELSE IF
EQU(FR1,"YARM") THEN BITS←YARM_MECH ELSE
ERROR("For force sensing can only use barm or yarm in move");
WHILE EQU(TOKEN,"ON") OR EQU(TOKEN,"WITH") DO
BEGIN RPTR(CLAUSE)C; C←NEW_RECORD(CLAUSE);
IF ERRCMON_SEEN THEN ERROR("Cant have any more clauses after ERROR clause,"&crlf&
"otherwise POINTY gets into trouble");
IF EQU(TOKEN,"ON")
THEN CMONPROC(MOVE_ST,C,BITS)
ELSE WITHPROC(MOVE_ST,C,BITS);
CLAUSES[#CLAUSES←#CLAUSES+1]←C;
IF CLAUSE:TYPE[C]=FAILURE_COND THEN ERRCMON_SEEN←TRUE;
END;
$$PCODE←FULLMOVE(CLAUSES, #CLAUSES,MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL);
$TMPOFF←TMPOFF;
END "on or with";
STOKEN←TRUE;
$TMPOFF←TMPOFF0;
END;
INTERNAL PROCEDURE PARKINGPROC;
BEGIN
STRING PAR;
GTOKEN(FALSE);
IF FINAL THEN ASKUSER("MOVE BARM TO BPARK; {MOVE YARM TO YPARK}")
ELSE IF EQU(TOKEN,"BARM") THEN ASKUSER("MOVE BARM TO BPARK")
ELSE IF EQU(TOKEN,"YARM") THEN ASKUSER("MOVE YARM TO YPARK")
ELSE ERROR("can only park BARM or YARM");
$$PCODE←PARSE;
END;
! drivecode,opclcode,jtmove,driveproc;
! drives the indicated joint of the arm (what): movement is absolute
if how=to, differential if how=by;
PROCEDURE DRIVECODE(STRING WHAT,HOW;INTEGER JOINT;RPTR(EXPR$)SCAL);
$$PCODE←$DRIVEPCODE((IF EQU(WHAT,"BJT") THEN BLUE
ELSE YELLOW),HOW,JOINT,SCAL);
! executes close or open instruction. How determines if the movement is
absolute (to) or differential (by), op indicates the operation(open/close);
INTERNAL PROCEDURE OPCLCODE(STRING OP,HAND,HOW;RPTR(EXPR$)SCAL);
BEGIN
IF EQU(HAND,"BHAND")
THEN IF EQU(HOW,"TO") OR EQU(OP,"OPEN")
THEN DRIVECODE("BJT",HOW,7,SCAL)
ELSE DRIVECODE("BJT",HOW,7,$APPEND(SCAL,$PCD1(XXSNEG),#SC))
ELSE PRINT(#NOTYET);
$DISPLAYLIST[#SC]←NULL;
END;
! parses the instruction
DRIVE BJT|YJT (#) TO|BY <scalar>;
INTERNAL PROCEDURE JTMOVE(STRING WHAT,HOW;INTEGER JOINT);
BEGIN "J"
RPTR(EXPR$) SCAL;
SCAL←$$GTANYEXP("joint movement angle",#SC);
OLDSAV("DRIVE",CVS(JOINT)); ! saves for default instructions;
IF EQU(WHAT,"BJT") THEN
DRIVECODE(WHAT,HOW,JOINT,SCAL)
ELSE PRINT(#NOTYET);
$DISPLAYLIST[#FR]←NULL;
END "J";
INTERNAL PROCEDURE DRIVEPROC;
BEGIN
STRING HOW;
STRING WHAT;INTEGER JOINT;
WHAT←IDF_READ;
IF EQU(WHAT,"BJT") OR EQU(WHAT,"YJT")
THEN BEGIN
WORD_READ("("); ! reads "(number)";
GTOKEN;
JOINT←INTSCAN(TOKEN,$BRCHR);
IF JOINT<1 OR JOINT>7
THEN ERROR("non existent joint: ",cvs(joint));
WORD_READ(")");
HOW←IDF_READ;
IF EQU(HOW,"BY") OR EQU(HOW,"TO")
THEN JTMOVE(WHAT,HOW,JOINT)
ELSE ERROR("TO or BY required");
END
ELSE ERROR("BJT or YJT required");
$DISPLAYLIST[#FR]←NULL;
END;
! centerproc,stopproc,retryproc;
INTERNAL RECURSIVE PROCEDURE CENTERPROC;
BEGIN RPTR(EXPR$)MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL; STRING FR1,AXIS;
STRING POS;
INTEGER TMPOFF0;
POS←ARM_READ; ! if the arm is not indicated BARM is assumed;
IF EQU(POS,"BARM")
THEN $$PCODE←MOVECODE←$CENTERPCODE(BLUE)
ELSE PRINT(#NOTYET);
TMPOFF0←$TMPOFF;
GTOKEN(FALSE);
IF EQU(TOKEN,"ON") OR EQU(TOKEN,"WITH")THEN
BEGIN "on or with"
RPTR(CLAUSE)ARRAY CLAUSES[1:15]; INTEGER #CLAUSES;
INTEGER BITS,TMPOFF;
TMPOFF←$TMPOFF; #CLAUSES←0;
IF EQU(POS,"BARM") THEN BITS←BARM_MECH+BHAND_MECH
ELSE BITS←YARM_MECH+YHAND_MECH;
WHILE EQU(TOKEN,"ON") OR EQU(TOKEN,"WITH") DO
BEGIN RPTR(CLAUSE)C; C←NEW_RECORD(CLAUSE);
IF EQU(TOKEN,"ON")
THEN CMONPROC(CENTER_ST,C,BITS)
ELSE WITHPROC(CENTER_ST,C,BITS);
CLAUSES[#CLAUSES←#CLAUSES+1]←C;
END;
$$PCODE←FULLMOVE(CLAUSES, #CLAUSES,MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL);
$TMPOFF←TMPOFF;
END "on or with";
STOKEN←TRUE;
$TMPOFF←TMPOFF0;
END;
INTERNAL PROCEDURE STOPPROC;
BEGIN "STOPPROC"
STRING POS;
POS←ARM_READ;
IF EQU(POS,"BARM")
THEN $$PCODE←$PCD11(XXSTOP,BARM_MECH)
ELSE PRINT(#NOTYET);
END "STOPPROC";
INTERNAL PROCEDURE RETRYPROC;
BEGIN "RETRYPROC"
IF NOT $ERRCMON THEN ERROR("RETRY: only valid inside an ERROR condition monitor");
IF ($ERRLEVEL≠$LEVEL) AND ($ERRLEVEL+1≠$LEVEL) THEN
ERROR("RETRY: must be the same lexical level as the block of theerror condition");
$$PCODE←$PCD1(XXPRETRY);
END "RETRYPROC";
! opening, opclproc,closeproc;
INTERNAL PROCEDURE OPENING(STRING FIRST,WHAT,HOW);
BEGIN
RPTR(EXPR$)SCAL;
SCAL←$$GTANYEXP("hand opening or closing",#SC);
OLDSAV(FIRST,WHAT); ! saves for default instructions;
OPCLCODE(FIRST,WHAT,HOW,SCAL);
END;
! parses the instructions
OPEN <hand> TO|BY <scalar>;
! CLOSE <hand> TO|BY <scalar>;
INTERNAL PROCEDURE OPCLPROC(STRING FIRST);
BEGIN
STRING WHAT;
WHAT←HAND_READ;
WORD2_READ("TO","BY");
OPENING(FIRST,WHAT,TOKEN);
END;
! parses the instructions
CLOSE <hand> TO|BY <scalar> (BHAND as default);
INTERNAL PROCEDURE CLOSEPROC;
BEGIN
STRING HAND,HOW;
GTOKEN;
IF EQU(HAND←TOKEN,"BHAND") OR EQU(TOKEN,"YHAND")
THEN GTOKEN
ELSE HAND←"BHAND";
IFλEQU(HOW←TOKEN,"BY") OR EQU(TOKEN,"TO")
THEN OPENING("CLOSE",HAND,HOW)
ELSE ERROR("CLOSE: need hand opening TO or BY");
END;
! onproc;
INTERNAL RECURSIVE PROCEDURE ONPROC(RPTR(SYMBOL)S(NULL_RECORD); BOOLEAN DEFER(FALSE));
BEGIN
INTEGER NBITS;RPTR(CLAUSE)CL; RPTR(RSTACK)R;
$COMPILE←$COMPILE+1;
IF S=NULL_RECORD THEN ERROR("Can only handle labelled cmon now");
CL←NEW_RECORD(CLAUSE);
GTOKEN;
IF EQU(TOKEN,"ERROR") THEN ERROR("ERROR CMON only valid in move statement")
ELSE
BEGIN
IF TOKEN="|" THEN
BEGIN GTOKEN;
IF EQU(TOKEN,"FORCE") THEN FORCECMON(CL,BARM_MECH,FORCE_COND,TRUE)
ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECMON(CL,BARM_MECH+'3000,
TORQUE_COND,TRUE)
ELSE ERROR("Must have FORCE or TORQUE after |");
END
ELSE IF EQU(TOKEN,"FORCE") THEN FORCECMON(CL,BARM_MECH,FORCE_COND)
ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECMON(CL,BARM_MECH+'3000,TORQUE_COND)
ELSE IF EQU(TOKEN,"DURATION") THEN DURCMON(CL)
ELSE IF (#TOKEN=ID_TYPE) AND (SYMBOL:TYPE[TOKENPTR]=#EV) THEN EVCMON(CL)
ELSE EXPCMON(CL);
R←NEW_RSTACK;
RPUSH(R,$ONPCODE(CLAUSE:HEADER[CL],SYMBOL:OFFSET[S],
CLAUSE:CMONCODE[CL],CLAUSE:FBITS[CL]));
IF CLAUSE:HEAD0[CL] THEN RPUSH(R,CLAUSE:HEAD0[CL]);
IF NOT DEFER THEN RPUSH(R,$PCD11(XXCMENBL,SYMBOL:OFFSET[S]));
$$PCODE←$RAPPEND(R);
END;
$COMPILE←$COMPILE-1;
END;
END "PPROC2";